# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "dataStruct.h"
# include "XmlParameterManager.h"

class tracking_base
{
protected:
    // 系统参数
    struct param 
    {
        data::vehicle_param m_veh;
        data::trajectory m_track;
    } m_param;

    // 是否循环跟踪当前轨迹
    bool loop = false;

    // 参考插值
    cpp_tools::gridInterp varphi_interp;
    cpp_tools::gridInterp curve_interp;
    cpp_tools::gridInterp psi_interp;
    cpp_tools::gridInterp gamma_interp;
    
    cpp_tools::gridInterp v_ref_interp;
    cpp_tools::gridInterp beta_ref_interp;
    cpp_tools::gridInterp e_ref_interp;
    cpp_tools::gridInterp Delta_phi_ref_interp;

    /// @brief 解析状态值向量，将其转化为linCurv和dynamic
    /// @param state 状态量[x,y,varphi,psi,gamma,v,beta,omega,a_x,a_y]
    /// @return {Frenet, Dynamic}
    virtual std::pair<data::Frenet,data::Dynamic> readStateVec(Eigen::VectorXd const & state)
    {
        int i = 0;
        double x = state(i++);
        double y = state(i++);
        double varphi = state(i++);
        double psi = state(i++);
        double gamma = state(i++);
        double v = state(i++);
        double beta = state(i++);
        double omega = state(i++);
        double a_x = state(i++);
        double a_y = state(i++);

        // 计算Frenet坐标系下的定位信息，如果当前没有可用路径则定位{0,0,0}
        data::Frenet linCurve = {0,0,0};
        auto [s,e] = cpp_tools::xy2sce_local(m_param.m_track,x,y);
        double road_varphi = varphi_interp.interp(s);
        double Delta_phi = varphi + beta - road_varphi;
        Delta_phi = cpp_tools::mod2pi(Delta_phi);

        // 为了稳定性，对e和Delta_phi进行cutoff
        e = cpp_tools::cutoff(e,0.001);
        Delta_phi = cpp_tools::cutoff(Delta_phi,0.000);
        
        linCurve = {s,e,Delta_phi};

        data::Dynamic dynamic = {
            v,
            beta,
            omega,
            a_x,
            a_y,
        };
        
        return {linCurve,dynamic};
    }

    /// @brief 获取参考轨迹
    virtual void readReference()
    {
        std::string settingsFile = host_path + settings_path + "/tracking/settings.xml";
        cpp_tools::XmlParameterManager m_xml("settings", settingsFile);
        std::string track_name = "";
        bool flag = m_xml.getParameter("", "referenceTrack", track_name);
        flag &= m_xml.getParameter("", "loop", loop);

        if (!flag || track_name.empty())
        {
            m_xml.setParameter("", "referenceTrack", track_name);
            m_xml.setParameter("", "loop", loop);
            m_xml.saveToFile(settingsFile);
            throw std::runtime_error("无效的设置， 请重新编辑文件：" + settingsFile);
        }

        if (loop == 0) // 水平外插
        {
            std::cout << "tracking controller: 单圈模式" << std::endl;
        }
        else if (loop == 1) // 循环外插
        {
            std::cout << "tracking controller: 循环模式" << std::endl;
        }
        
        // 读取参考轨迹
        auto trackData = cpp_tools::csvRead(host_path + track_path + track_name);
        if (trackData.size() != 0)
        {
            std::cout << "tracking controller: 成功获取参考轨迹" << track_name << std::endl;

            m_param.m_track.N = trackData.rows();

            int i = 0;
            m_param.m_track.x = trackData(Eigen::all,i++);
            m_param.m_track.y = trackData(Eigen::all,i++);
            m_param.m_track.varphi = trackData(Eigen::all,i++);
            m_param.m_track.S = trackData(Eigen::all,i++);
            m_param.m_track.curve = trackData(Eigen::all,i++);
            m_param.m_track.psi = trackData(Eigen::all,i++);
            m_param.m_track.gamma = trackData(Eigen::all,i++);
            m_param.m_track.v_ref = trackData(Eigen::all,i++);
            m_param.m_track.beta_ref = trackData(Eigen::all,i++);
            m_param.m_track.e_ref = trackData(Eigen::all,i++);
            m_param.m_track.Delta_phi_ref = trackData(Eigen::all,i++);
            
            auto S = m_param.m_track.S;
            varphi_interp.reset(S, m_param.m_track.varphi);
            curve_interp.reset(S, m_param.m_track.curve);
            psi_interp.reset(S, m_param.m_track.psi);
            gamma_interp.reset(S, m_param.m_track.gamma);

            v_ref_interp.reset(S, m_param.m_track.v_ref);
            beta_ref_interp.reset(S, m_param.m_track.beta_ref);
            e_ref_interp.reset(S, m_param.m_track.e_ref);
            Delta_phi_ref_interp.reset(S, m_param.m_track.Delta_phi_ref);
        }
        else
        {
            throw std::runtime_error("加载参考路径失败，找不到文件: " + host_path + track_path + track_name);
        }
    };
public:
    tracking_base()
    {
        
    };

    ~tracking_base()
    {

    };

    /// @brief 调用该函数用于提前停止对象工作
    virtual void shutdown()
    {

    };

    /// @brief 调用该函数更新控制器的控制量，一般在状态量更新后调用
    /// @param state 状态量[x,y,varphi,psi,gamma,v,beta,omega,a_x,a_y]
    /// @param delta 前轮转角
    /// @param F_x 总纵向力输出
    /// @param time 传感器更新时间
    virtual void update(Eigen::VectorXd const & state, double delta, double F_x, double time)
    {
        
    };

    /// @brief 调用该函数获取控制器内的当前控制量
    /// @return {delta, F_x}
    virtual std::pair<double,double> getControl(Eigen::VectorXd const & state, double delta, double F_x, double time) = 0;

    /// @brief 获取计算求解时间
    /// @return 
    virtual double getComputTime()
    {
        return -1;
    };

    /// @brief 刷新
    virtual int refresh()
    {
        std::cout << "你按下了刷新键，但是控制器并不会刷新" << std::endl;

        return 0;
    };
};
